OTG接口介绍及使用示例

[TOC]

1 简介

OTG: Online Trajectory Generation, 在线轨迹生成算法。可以针对多自由度系统从任意的初始状态，受最大的速度、加速度和加加速度限制下，规划出到任意目标状态的时间最优轨迹，具有实时计算能力。计算结果是位置、速度、加速度和加加速度随时间变化的曲线，例如下图所示：

2 实例化对象及速度规划接口

2.1 调用如下接口，实例化otg对象

/**
     * @brief 构造函数
     * @param dim: 规划的自由度个数
     * @param cycleTimeInSeconds: 规划周期
     * @param type: OTG算法的类型，可使用默认值
     * @param mode: OTG算法的运动模式，分为位置模式和速度模式
     * @param sync: OTG算法的多自由度运动同步模式，包含相位同步、时间同步、不同步，可使用默认值
     */
    OTGInterface(const unsigned int& dim, const double& cycleTimeInSeconds, const OTGType& type = OTGType::TypeV,
                 const MoveMode& mode = MoveMode::POSITION, const SyncBehavior& sync = SyncBehavior::PHASE_SYNCHRONIZATION_IF_POSSIBLE);

2.2 调用如下接口，设置运动参数，计算速度轮廓，一般在实例化对象后调用

/**
     * @brief 设置运动参数, 计算速度轮廓
     * @param maxV: 运动最大速度约束
     * @param minV: 运动最小速度约束
     * @param maxA: 运动最大加速度约束
     * @param minA: 运动最小加速度约束
     * @param maxJ: 运动最大加加速度约束
     * @param curP: 当前运动位置
     * @param curV: 当前运动速度
     * @param curA: 当前运动加速度
     * @param tarP: 目标运动位置
     * @param tarV: 目标运动速度
     * @param tarA: 目标运动加速度
     * @param moveTime: 用户设置的整个运动过程时间，实际的运动时间不小于设定值
     * @return if < 0, 表示运动计算失败
     */
    int setMotionParameters(const double* maxV, const double* minV, const double* maxA, const double* minA,const double* maxJ, const double* curP, const double* curV, const double* curA,const double* targetP, const double* targetV, const double* targetA, const double moveTime = 0);

2.3 调用如下接口，单独设置速度规划的约束条件，会触发重新计算速度轮廓，适用于运动约束动态变化的场景

/**
     * @brief 设置速度规划的约束条件(会触发重新计算速度轮廓, 适用于运动约束动态变化的场景）
     * @param maxVel: 最大速度
     * @param maxAcc: 最大加速度
     * @param maxJ:   最大加加速度
     * @return if < 0, 表示速度规划失败
     */
    int setConstraints(const double* maxVel, const double* maxAcc, const double* maxJ);

2.4 调用如下接口，更新目标状态，会触发重新计算速度轮廓, 适用于目标跟踪场景

/**
     * @brief 更新目标状态(会触发重新计算速度轮廓, 适用于目标跟踪场景)
     * @param tarPos: 目标位置
     * @param tarVel: 目标速度
     * @param tarAcc: 目标加速度
     * @return if < 0, 表示速度规划失败
     */
    int updateTargetStates(const double* tarPos, const double* tarVel, const double* tarAcc);

3 采样接口

3.1 调用如下接口，按指定的时间间隔插值得到位置、速度、加速度

/**
     * @brief 按指定的时间间隔插值得到位置、速度、加速度信息，时间单位：s
     * @param posTraj: 采样位置
     * @param velTraj: 采样速度
     * @param accTraj: 采样加速度
     * @param stepTime:采样时间间隔
     * @return　if = 0, 表示正常采样，if = 1, 表示采到终点，采样结束
     */
    int otgStep(double* posTraj, double* velTraj, double* accTraj, const double stepTime = 0);

3.2 调用如下接口，计算给定时刻的轨迹信息

/**
     * @brief 计算在某个时刻的位置,速度和加速度
     * @param timeValueInSeconds: 给定时刻
     * @param posTraj: 位置
     * @param velTraj: 速度
     * @param accTraj: 加速度
     * @return if = 0, 表示正常采样，if = 1, 表示采到终点，采样结束
     */
    int otgPVA(const double& timeValueInSeconds, double* posTraj, double* velTraj, double* accTraj);

4 速度轮廓信息获取

/**
     * @brief 指定某个自由度，指定位置，获取对应的时间t, 速度v, 和加速度a（调用该函数时需保证位置随着时间单调递增）
     * @param index: 指定第index个维度
     * @param S: 给定一系列位置点
     * @param T: 获取对应的时间点
     * @param V: 获取对应的速度
     * @param A: 获取对应的加速度
     * @return if < 0, 表示计算失败
     */
    int calTVAGivenPos(const int index, const std::vector<double>& S, std::vector<double>& T, std::vector<double>& V, std::vector<double>& A);

5 使用示例

//！ 设置运动参数
std::vector<double> pos1, vel1, acc1, posT, velT, accT, maxV, maxA, minA, maxJ;
if(type == 1)
{
    //起始状态
    pos1 = {0.0, 0.0};
    vel1 = {2.13378, 0.682608};
    acc1 = {4.073, 0.0};

    //目标状态
    posT = {0.456938, 0.290321};
    velT = {0.0, 0.0};
    accT = {0.0, 0.0};

    //运动约束
    maxV = {0.9, 0.9};
    maxA = {10, 15};
    maxJ = {320, 48};
}

//！实例化otg对象
int DOF = 2;
double sample_period = 0.0005; 
otg::OTGInterfacePtr otg(new otg::OTGInterface(DOF, sample_period, otg::OTGType::TypeV, otg::MoveMode::POSITION, otg::SyncBehavior::PHASE_SYNCHRONIZATION_IF_POSSIBLE));

//！设置运动参数，计算速度轮廓
int ret = otg->setMotionParameters(maxV.data(), nullptr, maxA.data(), nullptr, maxJ.data(), pos1.data(), vel1.data(), acc1.data(), posT.data(), velT.data(), accT.data());

//！采样，可以使用otgStep和otgPVA任意一种,下面提供了使用示例
#if 0
//otgPVA使用示例
int dof = otg->getDimension();
double duration = otg->getDuration();
double sample_period = 0.005;
std::vector<std::vector<double>> T, P, V, A;
std::vector<double> posTraj(dof), velTraj(dof), accTraj(dof);
double time = 0;
do
{
    otg->otgPVA(time, posTraj.data(), velTraj.data(), accTraj.data());
    for(int i = 0; i < dof; i++)
    {
        P[i].push_back(posTraj[i]);
        V[i].push_back(velTraj[i]);
        A[i].push_back(accTraj[i]);
    }
    T.push_back(time);
    if(time == duration)
    break;

    time += sample_period;
    if(time > duration)
    time = duration;
}
while (time <= duration);

#else
//otgStep使用示例
double  duration = otg->getDuration();
double sample_period = 0.005;
std::vector<double> time_series;
std::vector<double> S[DOF], V[DOF], A[DOF];
double posTraj[DOF];
double velTraj[DOF];
double accTraj[DOF];

double total_time = 0;
double step_time = 0;
int time_num = std::floor(duration / sample_period);
for(int i = 0; i <= time_num + 1; i++)
{
    if(i == 0)
    {
        step_time = 0;
    }
    else if(i == time_num + 1)
    {
        step_time = duration - sample_period * time_num;
    }
    else
    {
        step_time = sample_period;
    }

    total_time = total_time + step_time;
    otg->otgStep(posTraj, velTraj, accTraj, step_time);

    time_series.push_back(total_time);
    for(int j = 0; j < DOF; j++)
    {
        S[j].push_back(posTraj[j]);
        V[j].push_back(velTraj[j]);
        A[j].push_back(accTraj[j]);
    }
}
#endif

//! 测试给定位置，计算对应的时间、速度和加速度
std::vector<double> pos = {pos1[0], (pos1[0] + posT[0]) / 2, posT[0]}, tt, vel, acc;
ret = otg->calTVAGivenPos(0, pos, tt, vel, acc);

//! 设置速度规划的约束条件,触发重新计算速度轮廓
maxV = {0.5, 1.0};    
ret = otg->setConstraints(maxV.data(), nullptr nullptr);    //只有最大速度发生改变

//！更新目标状态, 触发重新计算速度轮廓
posT = {0.856938, 0.590321};
velT = {0.0, 0.0};
accT = {0.0, 0.0};
ret = otg->updateTargetStates(posT.data(), velT.data(), accT.data());